/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/include/agvNavigate/helpInc/mark_check.h
 * @Description  : 定义MarkCheck类，用于处理二维码检测和管理运行数据。 
 *                 包含二维码丢失的检测逻辑、实时数据更新和线程管理。 
 *                 该类支持初始化、使能检测、添加新路径、清除检测缓冲区等功能。 
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 16:46:57
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/ 
#pragma once
#include "z_types.h"
#include "rclcpp/rclcpp.hpp"
#include <mutex>
#include <thread>
#include <vector>
#include <map>
#include "trajectory.h"
#include "path.h"
#include "tools.h"
#include <string>
#include <geometry_msgs/msg/pose.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <agv_srvs/srv/set_navigation_mode.hpp>
#include "timestamp.h"
//////////////////////////////////////////////////////////////////////////////
//   The class of class "RunData".
struct RunData {
    float progress_percent; // 当前路径的进度百分比
    float abs_progress;      // 当前路径的绝对距离
    USHORT from_node;        // 路径起点
    USHORT to_node;          // 路径终点

    RunData() {
        Clear();
    }

    void Clear() {
        progress_percent = 0.0f;
        abs_progress = 0.0f;
        from_node = 65535;
        to_node = 65535;
    }

    void Update(USHORT from_node, USHORT to_node, float abs_progress, float progress_percent) {
        this->from_node = from_node;
        this->to_node = to_node;
        this->abs_progress = abs_progress;
        this->progress_percent = progress_percent;
    }
};

//   The class  of class "MarkCheck".
class MarkCheck
{
public:
    // The constructor
    MarkCheck();
    ~MarkCheck();

    bool Init();                 // 初始化函数
    bool Trigger();              // 报丢失二维码;超过允许丢失的最大节点数量
    bool Trigger(std::string& miss_id); // 重载函数;报丢失二维码;超过允许丢失的最大节点数量
    void SetEnable(bool enable); // 使能检测
    bool AddNewPath(Path* path, Trajectory* traj); // 添加检测新的二维码ID
    void UpdateProgRunData(USHORT from_node, USHORT to_node, float abs_progress, float progress_percent); // 更新实时数据
    void ClearCheckBuffer();     // 清除检测缓冲区

private:
    void UpdateParam();           // 更新参数
    void Reset();                 // 复位清除当前状态
    void CycleRoutine();          // 检测二维码丢失逻辑
    void ThreadFunction();        // 线程函数
    void GetLandMarkCallback(const geometry_msgs::msg::PoseStamped& mark_data); // 获取二维码ID 回调函数
    USHORT GetNodeQrId(USHORT node_id); // 通过节点ID获取二维码ID
    bool IsNodeValid();           // 节点数据有效性
    bool IsIdSame(USHORT id1, USHORT id2); // id是否相同
    bool IsIdValid(USHORT id);    // id有效性
    void LogCheckIdBuffer();      // 打印当前检测二维码id缓冲区数据
    void CheckOneId();            // 检测缓冲区存在id的情况，缓冲区数量2
    void CheckTwoId();            // 缓冲区数量2
    void CheckOtherSizeId();      // 缓冲区数量ID错误

    bool initialized_;            // 初始化标识
    rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr landmark_subscriber_; // 订阅二维码ID
    RunData current_run_data_;    // 当前实时运行时的路段数据
    DataTimeStamp<USHORT> current_mark_id_; // 当前实时更新的二维码id
    std::thread thread_;          // 线程句柄
    std::timed_mutex quit_mutex_; // 线程退出互斥锁
    std::vector<USHORT> check_id_buffer_; // 待检测二维码ID缓冲区
    std::vector<USHORT> missed_id_buffer_; // 丢失的二维码ID缓冲区
    std::mutex mutex_;            // 数据操作互斥锁
    bool enabled_;                // 使能标识
    float max_miss_distance_;     // 最大丢失二维码允许走的距离 默认0.3m
    int max_miss_code_count_;     // 最大允许丢失的二维码数量
};
